/****************************************************************************
 *
 * Copyright 2016 Samsung Electronics All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 *
 ****************************************************************************/
/****************************************************************************
 * File: libc/string/lib_vikmemcpy.c
 *
 * This is version of the optimized memcpy by Daniel Vik, adapted to the
 * NuttX environment.
 *
 *   Copyright (C) 1999-2010 Daniel Vik
 *
 * Adaptations include:
 * - File name change
 * - Use of types defined in stdint.h
 * - Integration with the NuttX configuration system
 * - Other cosmetic changes for consistency with NuttX coding standards
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any
 * damages arising from the use of this software.
 * Permission is granted to anyone to use this software for any
 * purpose, including commercial applications, and to alter it and
 * redistribute it freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you
 *    must not claim that you wrote the original software. If you
 *    use this software in a product, an acknowledgment in the
 *    use this software in a product, an acknowledgment in the
 *    product documentation would be appreciated but is not
 *    required.
 *
 * 2. Altered source versions must be plainly marked as such, and
 *    must not be misrepresented as being the original software.
 *
 * 3. This notice may not be removed or altered from any source
 *    distribution.
 *
 * Description: Implementation of the standard library function memcpy.
 *              This implementation of memcpy() is ANSI-C89 compatible.
 *
 * The following configuration options can be set:
 *
 *   CONFIG_ENDIAN_BIG
 *     Uses processor with big endian addressing. Default is little endian.
 *
 *   CONFIG_MEMCPY_PRE_INC_PTRS
 *     Use pre increment of pointers. Default is post increment of pointers.
 *
 *   CONFIG_MEMCPY_INDEXED_COPY
 *     Copying data using array indexing. Using this option, disables the
 *     CONFIG_MEMCPY_PRE_INC_PTRS option.
 *
 *   CONFIG_MEMCPY_64BIT - Compiles memcpy for 64 bit architectures
 *
 ****************************************************************************/

/********************************************************************
 * Included Files
 *******************************************************************/

#include <tinyara/config.h>
#include <tinyara/compiler.h>

#include <stddef.h>
#include <stdint.h>
#include <string.h>

/********************************************************************
 * Pre-processor Definitions
 *******************************************************************/

/* Can't support CONFIG_MEMCPY_64BIT if the platform does not have 64-bit
 * integer types.
 */

#ifndef CONFIG_HAVE_LONG_LONG
#undef CONFIG_MEMCPY_64BIT
#endif

/* Remove definitions when CONFIG_MEMCPY_INDEXED_COPY is defined */

#if defined(CONFIG_MEMCPY_INDEXED_COPY)
#if defined(CONFIG_MEMCPY_PRE_INC_PTRS)
#undef CONFIG_MEMCPY_PRE_INC_PTRS
#endif							/* CONFIG_MEMCPY_PRE_INC_PTRS */
#endif							/* CONFIG_MEMCPY_INDEXED_COPY */

/* Definitions for pre and post increment of pointers */

#if defined(CONFIG_MEMCPY_PRE_INC_PTRS)

#define START_VAL(x)            (x)--
#define INC_VAL(x)              *++(x)
#define CAST_TO_U8(p, o)        ((uint8_t*)p + o + TYPE_WIDTH)
#define WHILE_DEST_BREAK        (TYPE_WIDTH - 1)
#define PRE_LOOP_ADJUST         - (TYPE_WIDTH - 1)
#define PRE_SWITCH_ADJUST       + 1

#else							/* CONFIG_MEMCPY_PRE_INC_PTRS */

#define START_VAL(x)
#define INC_VAL(x)              *(x)++
#define CAST_TO_U8(p, o)        ((uint8_t*)p + o)
#define WHILE_DEST_BREAK        0
#define PRE_LOOP_ADJUST
#define PRE_SWITCH_ADJUST

#endif							/* CONFIG_MEMCPY_PRE_INC_PTRS */

/* Definitions for endian-ness */

#ifdef CONFIG_ENDIAN_BIG

#define SHL <<
#define SHR >>

#else							/* CONFIG_ENDIAN_BIG */

#define SHL >>
#define SHR <<

#endif							/* CONFIG_ENDIAN_BIG */

/********************************************************************
 * Macros for copying words of  different alignment.
 * Uses incremening pointers.
 *******************************************************************/

#define CP_INCR()                         \
	do {                                  \
		INC_VAL(dstN) = INC_VAL(srcN);    \
	} while (0)

#define CP_INCR_SH(shl, shr)              \
	do {                                  \
		dstWord       = srcWord SHL shl;  \
		srcWord       = INC_VAL(srcN);    \
		dstWord      |= srcWord SHR shr;  \
		INC_VAL(dstN) = dstWord;          \
	} while (0)

/********************************************************************
 * Macros for copying words of  different alignment.
 * Uses array indexes.
 *******************************************************************/

#define CP_INDEX(idx)                     \
	do {                                  \
		dstN[idx] = srcN[idx];            \
	} while (0)

#define CP_INDEX_SH(x, shl, shr)          \
	do {                                  \
		dstWord   = srcWord SHL shl;      \
		srcWord   = srcN[x];              \
		dstWord  |= srcWord SHR shr;      \
		dstN[x]   = dstWord;              \
	} while (0)

/********************************************************************
 * Macros for copying words of different alignment.
 * Uses incremening pointers or array indexes depending on
 * configuration.
 *******************************************************************/

#if defined(CONFIG_MEMCPY_INDEXED_COPY)

#define CP(idx)               CP_INDEX(idx)
#define CP_SH(idx, shl, shr)  CP_INDEX_SH(idx, shl, shr)

#define INC_INDEX(p, o)       ((p) += (o))

#else							/* CONFIG_MEMCPY_INDEXED_COPY */

#define CP(idx)               CP_INCR()
#define CP_SH(idx, shl, shr)  CP_INCR_SH(shl, shr)

#define INC_INDEX(p, o)

#endif							/* CONFIG_MEMCPY_INDEXED_COPY */

#define COPY_REMAINING(count)                                     \
	do {                                                          \
		START_VAL(dst8);                                          \
		START_VAL(src8);                                          \
																  \
		switch (count) {                                          \
		case 7: INC_VAL(dst8) = INC_VAL(src8);                    \
		case 6: INC_VAL(dst8) = INC_VAL(src8);                    \
		case 5: INC_VAL(dst8) = INC_VAL(src8);                    \
		case 4: INC_VAL(dst8) = INC_VAL(src8);                    \
		case 3: INC_VAL(dst8) = INC_VAL(src8);                    \
		case 2: INC_VAL(dst8) = INC_VAL(src8);                    \
		case 1: INC_VAL(dst8) = INC_VAL(src8);                    \
		case 0:                                                   \
		default: break;                                           \
		}                                                         \
	} while (0)

#define COPY_NO_SHIFT()                                           \
	do {                                                          \
		UIntN* dstN = (UIntN*)(dst8 PRE_LOOP_ADJUST);             \
		UIntN* srcN = (UIntN*)(src8 PRE_LOOP_ADJUST);             \
		size_t length = count / TYPE_WIDTH;                       \
																  \
		while (length & 7) {                                      \
			CP_INCR();                                            \
			length--;                                             \
		}                                                         \
																  \
		length /= 8;                                              \
																  \
		while (length--) {                                        \
			CP(0);                                                \
			CP(1);                                                \
			CP(2);                                                \
			CP(3);                                                \
			CP(4);                                                \
			CP(5);                                                \
			CP(6);                                                \
			CP(7);                                                \
																  \
			INC_INDEX(dstN, 8);                                   \
			INC_INDEX(srcN, 8);                                   \
		}                                                         \
																  \
		src8 = CAST_TO_U8(srcN, 0);                               \
		dst8 = CAST_TO_U8(dstN, 0);                               \
																  \
		COPY_REMAINING(count & (TYPE_WIDTH - 1));                 \
																  \
		return dest;                                              \
	} while (0)

#define COPY_SHIFT(shift)                                         \
	do {                                                          \
		UIntN *dstN  = (UIntN *)((((uintptr_t)dst8) PRE_LOOP_ADJUST) & \
								 ~(TYPE_WIDTH - 1));              \
		UIntN *srcN  = (UIntN *)((((uintptr_t)src8) PRE_LOOP_ADJUST) & \
								 ~(TYPE_WIDTH - 1));              \
		size_t length  = count / TYPE_WIDTH;                      \
		UIntN srcWord = INC_VAL(srcN);                            \
		UIntN dstWord;                                            \
																  \
		while (length & 7) {                                      \
			CP_INCR_SH(8 * shift, 8 * (TYPE_WIDTH - shift));      \
			length--;                                             \
		}                                                         \
																  \
		length /= 8;                                              \
																  \
		while (length--) {                                        \
			CP_SH(0, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
			CP_SH(1, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
			CP_SH(2, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
			CP_SH(3, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
			CP_SH(4, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
			CP_SH(5, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
			CP_SH(6, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
			CP_SH(7, 8 * shift, 8 * (TYPE_WIDTH - shift));        \
																  \
			INC_INDEX(dstN, 8);                                   \
			INC_INDEX(srcN, 8);                                   \
		}                                                         \
																  \
		src8 = CAST_TO_U8(srcN, (shift - TYPE_WIDTH));            \
		dst8 = CAST_TO_U8(dstN, 0);                               \
																  \
		COPY_REMAINING(count & (TYPE_WIDTH - 1));                 \
																  \
		return dest;                                              \
	} while (0)

/********************************************************************
 * Type Definitions
 *******************************************************************/

#ifdef CONFIG_MEMCPY_64BIT
typedef uint64_t UIntN;
#define TYPE_WIDTH        8L
#else
typedef uint32_t UIntN;
#define TYPE_WIDTH        4L
#endif

/********************************************************************
 * Public Functions
 *******************************************************************/
/********************************************************************
 * Name: memcpy
 *
 * Description:
 *   Copies count bytes from src to dest. No overlap check is performed.
 *
 * Input Parameters:
 *   dest        - pointer to destination buffer
 *   src         - pointer to source buffer
 *   count       - number of bytes to copy
 *
 * Returned Value:
 *   A pointer to destination buffer
 *
 *******************************************************************/

void *memcpy(void *dest, const void *src, size_t count)
{
	uint8_t *dst8 = (uint8_t *)dest;
	uint8_t *src8 = (uint8_t *)src;

	if (count < 8) {
		COPY_REMAINING(count);
		return dest;
	}

	START_VAL(dst8);
	START_VAL(src8);

	while (((uintptr_t)dst8 & (TYPE_WIDTH - 1)) != WHILE_DEST_BREAK) {
		INC_VAL(dst8) = INC_VAL(src8);
		count--;
	}

	switch ((((uintptr_t)src8) PRE_SWITCH_ADJUST) & (TYPE_WIDTH - 1)) {
	case 0:
		COPY_NO_SHIFT();
		break;
	case 1:
		COPY_SHIFT(1);
		break;
	case 2:
		COPY_SHIFT(2);
		break;
	case 3:
		COPY_SHIFT(3);
		break;
#if TYPE_WIDTH > 4
	case 4:
		COPY_SHIFT(4);
		break;
	case 5:
		COPY_SHIFT(5);
		break;
	case 6:
		COPY_SHIFT(6);
		break;
	case 7:
		COPY_SHIFT(7);
		break;
#endif
	}

	return dest;
}
